﻿using HalconDotNet;
using IniReadWrite;
using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Xml.Serialization;

namespace VisionRoc.Core
{
    /// <summary>
    /// 下相机旋转标定
    /// </summary>
    public class RotateCorrectAlgo
    {

        public CalibrationData m_CalibrationData;

        /// <summary>
        /// 下相机纠偏参INI文件参数调试路径
        /// </summary>
        public string RotateCorrectPath;

        /// <summary>
        /// 标定文件路径
        /// </summary>
        public string NineCalibrationFilePath;

        /// <summary>
        /// 旋转标定文件路径
        /// </summary>
        public string RotateCalibrationFilePath;

        public RotateCorrectAlgo(string _Path)
        {
            try
            {
                int state = 0;
                m_CalibrationData = new CalibrationData();
                //判断文件路径是否存在
                if (!Directory.Exists(_Path))
                {
                    Directory.CreateDirectory(_Path);
                }
                RotateCorrectPath = _Path + "\\RotateParameter.ini";
                if (File.Exists(RotateCorrectPath))
                {
                    ReadParameter();
                    state += 1;
                }

                //判断标定文件是存在
                if (File.Exists(NineCalibrationFilePath))
                {
                    ReafCalibrationXML(NineCalibrationFilePath);
                    //HomMat2D = RotateCalculateAlgo.GenHomMat2D(m_CalibrationData.SX, m_CalibrationData.SY, m_CalibrationData.Theta, m_CalibrationData.Phi, m_CalibrationData.TX, m_CalibrationData.TY);
                    state += 1;
                }

                //判断旋转标定文件路径是否存在
                if (File.Exists(RotateCalibrationFilePath))
                {
                    ReafCalibrationXML(RotateCalibrationFilePath);
                    RotateCenterX = Math.Round(m_CalibrationData.ImageRotatoCenterX, 3);
                    RotateCenterY = Math.Round(m_CalibrationData.ImageRotatoCenterY, 3);
                    RotateRadius = Math.Round(m_CalibrationData.ImageRotatoRadius, 3);
                    state += 1;
                }

                if (state == 3)
                {
                    InIState = 1;
                }
                else
                {
                    InIState = -1;
                }

            }
            catch
            {
                InIState = -1;
            }
        }

        /// <summary>
        /// 类初始化状态  1为初始化成功  -1为初始化失败
        /// </summary>
        public int InIState;

        #region 示教点变量

        /// <summary>
        /// 旋转中心ROW
        /// </summary>
        public double RotateCenterX;

        /// <summary>
        /// 旋转中心Col
        /// </summary>
        public double RotateCenterY;

        /// <summary>
        /// 旋转中心半径
        /// </summary>
        public double RotateRadius;


        /// <summary>
        /// 下相机纠偏基准坐标 X
        /// </summary>
        public double ModelX;

        /// <summary>
        /// 下相机纠偏基准坐标 Y
        /// </summary>
        public double ModelY;

        /// <summary>
        /// 下相机目标角度
        /// </summary>
        public double ModelR;

        #endregion

        /// <summary>
        /// AngleDiffSet = 1 :目标角度-实时角度=角度差
        /// AngleDiffSet = 2 :实时角度-目标角度=角度差
        /// </summary>
        public int AngleDiffSet;

        /// <summary>
        ///  ResuleAngleIsOrReverse=1 ： 结果正常发送
        ///  ResuleAngleIsOrReverse=2 ： 结果取反发送
        /// </summary>
        public int ResuleAngleIsOrReverse;

        public HTuple HomMat2D = new HTuple();

        public void ReafCalibrationXML(string Path)
        {
            using (FileStream fs = new FileStream(Path, FileMode.Open))
            {
                XmlSerializer xml = new XmlSerializer(typeof(CalibrationData));
                m_CalibrationData = xml.Deserialize(fs) as CalibrationData;
                fs.Close();
            }
        }

        /// <summary>
        /// 旋转纠偏公式Halcon算法  OK
        /// </summary>
        /// <param name="Angle">当前角度</param>
        /// <param name="Row">当前坐标Row</param>
        /// <param name="Col">当前坐标Col</param>
        /// <param name="NewRow">旋转后新坐标Row</param>
        /// <param name="NewCol">旋转后新坐标Row</param>
        /// <param name="OffseAngle">角度偏差</param>
        public bool RotateAlgo1(double Angle, double Row, double Col, out double NewRow, out double NewCol, out double OffseAngle)
        {
            try
            {
                double AngleNow = 0;
                NewRow = 0;
                NewCol = 0;
                OffseAngle = 0;
                if (InIState == 3)
                {
                    if (AngleDiffSet == 2)
                    {
                        //角度差=当前角度 - 目标角度
                        AngleNow = Angle - ModelR;
                    }
                    else if (AngleDiffSet == 1)
                    {
                        //角度差 = 目标角度-当前角度 1944 3045
                        AngleNow = ModelR - Angle;
                    }
                    //RotateCalculateAlgo.Rotate1(RotateCenterX, RotateCenterY, AngleNow, Row, Col, out NewRow, out NewCol, out OffseAngle);
                    return true;
                }
                else
                {
                    return false;
                }

            }
            catch
            {
                NewRow = 0;
                NewCol = 0;
                OffseAngle = 0;
                return false;
            }
        }

        /// <summary>
        /// 旋转纠偏公式 公式计算 OK
        /// </summary>
        /// <param name="Angle">当前角度</param>
        /// <param name="Row">当前坐标Row</param>
        /// <param name="Col">当前坐标Col</param>
        /// <param name="NewRow">旋转后坐标Row</param>
        /// <param name="NewCol">旋转后坐标Col</param>
        /// <param name="OffseAngle">角度偏差</param>
        public void RotateAlgo2(double Angle, double Row, double Col, out double NewRow, out double NewCol, out double OffseAngle)
        {
            try
            {
                double AngleNow = 0;
                NewRow = 0;
                NewCol = 0;
                OffseAngle = 0;
                if (AngleDiffSet == 2)
                {
                    //角度差=当前角度 - 目标角度
                    AngleNow = Angle - ModelR;
                }
                else if (AngleDiffSet == 1)
                {
                    //角度差 = 目标角度-当前角度
                    AngleNow = ModelR - Angle;
                }

                //RotateCalculateAlgo.Rotate2(RotateCenterX, RotateCenterY, AngleNow, Row, Col, out NewRow, out NewCol, out OffseAngle);
            }
            catch
            {
                NewRow = 0;
                NewCol = 0;
                OffseAngle = 0;
            }
        }

        /// <summary>
        /// 保存旋转纠偏参数
        /// </summary>
        public void WriteParameter()
        {
            IniAPI.INIWriteValue(RotateCorrectPath, "RotateParameter", "ModelX", ModelX.ToString());
            IniAPI.INIWriteValue(RotateCorrectPath, "RotateParameter", "ModelY", ModelY.ToString());
            IniAPI.INIWriteValue(RotateCorrectPath, "RotateParameter", "ModelAngle", ModelR.ToString());
            IniAPI.INIWriteValue(RotateCorrectPath, "RotateParameter", "AngleDiffSet", AngleDiffSet.ToString());
            IniAPI.INIWriteValue(RotateCorrectPath, "RotateParameter", "ResuleAngleIsOrReverse", ResuleAngleIsOrReverse.ToString());

            IniAPI.INIWriteValue(RotateCorrectPath, "RotateParameter", "CalibrationFilePath", NineCalibrationFilePath.ToString());
            IniAPI.INIWriteValue(RotateCorrectPath, "RotateParameter", "RotateCalibrationFilePath", RotateCalibrationFilePath.ToString());
        }

        /// <summary>
        /// 读取旋转纠偏参数
        /// </summary>
        public void ReadParameter()
        {

            ModelX = Convert.ToDouble(IniAPI.INIGetStringValue(RotateCorrectPath, "RotateParameter", "ModelX", "0"));
            ModelY = Convert.ToDouble(IniAPI.INIGetStringValue(RotateCorrectPath, "RotateParameter", "ModelY", "0"));
            ModelR = Convert.ToDouble(IniAPI.INIGetStringValue(RotateCorrectPath, "RotateParameter", "ModelAngle", "0"));
            AngleDiffSet = Convert.ToInt32(IniAPI.INIGetStringValue(RotateCorrectPath, "RotateParameter", "AngleDiffSet", "0"));
            ResuleAngleIsOrReverse = Convert.ToInt32(IniAPI.INIGetStringValue(RotateCorrectPath, "RotateParameter", "ResuleAngleIsOrReverse", "0"));

            NineCalibrationFilePath = IniAPI.INIGetStringValue(RotateCorrectPath, "RotateParameter", "CalibrationFilePath", "0");
            RotateCalibrationFilePath = IniAPI.INIGetStringValue(RotateCorrectPath, "RotateParameter", "RotateCalibrationFilePath", "0");
        }

    }
}
